
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       uitc_estimator_status.h
  * @author     baiyang
  * @date       2022-3-13
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <uITC/uITC.h>
/*-----------------------------------macro------------------------------------*/
ITC_DECLARE(estimator_status);

#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_GPS_FIX 0
#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_MIN_SAT_COUNT 1
#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_MAX_PDOP 2
#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_MAX_HORZ_ERR 3
#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_MAX_VERT_ERR 4
#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_MAX_SPD_ERR 5
#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_MAX_HORZ_DRIFT 6
#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_MAX_VERT_DRIFT 7
#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR 8
#define ESTIMATOR_STATUS_GPS_CHECK_FAIL_MAX_VERT_SPD_ERR 9
#define ESTIMATOR_STATUS_CS_TILT_ALIGN 0
#define ESTIMATOR_STATUS_CS_YAW_ALIGN 1
#define ESTIMATOR_STATUS_CS_GPS 2
#define ESTIMATOR_STATUS_CS_OPT_FLOW 3
#define ESTIMATOR_STATUS_CS_MAG_HDG 4
#define ESTIMATOR_STATUS_CS_MAG_3D 5
#define ESTIMATOR_STATUS_CS_MAG_DEC 6
#define ESTIMATOR_STATUS_CS_IN_AIR 7
#define ESTIMATOR_STATUS_CS_WIND 8
#define ESTIMATOR_STATUS_CS_BARO_HGT 9
#define ESTIMATOR_STATUS_CS_RNG_HGT 10
#define ESTIMATOR_STATUS_CS_GPS_HGT 11
#define ESTIMATOR_STATUS_CS_EV_POS 12
#define ESTIMATOR_STATUS_CS_EV_YAW 13
#define ESTIMATOR_STATUS_CS_EV_HGT 14
#define ESTIMATOR_STATUS_CS_BETA 15
#define ESTIMATOR_STATUS_CS_MAG_FIELD 16
#define ESTIMATOR_STATUS_CS_FIXED_WING 17
#define ESTIMATOR_STATUS_CS_MAG_FAULT 18
#define ESTIMATOR_STATUS_CS_ASPD 19
#define ESTIMATOR_STATUS_CS_GND_EFFECT 20
#define ESTIMATOR_STATUS_CS_RNG_STUCK 21
#define ESTIMATOR_STATUS_CS_GPS_YAW 22
#define ESTIMATOR_STATUS_CS_MAG_ALIGNED 23
#define ESTIMATOR_STATUS_CS_EV_VEL 24
#define ESTIMATOR_STATUS_CS_SYNTHETIC_MAG_Z 25
#define ESTIMATOR_STATUS_CS_VEHICLE_AT_REST 26
#define ESTIMATOR_STATUS_CS_GPS_YAW_FAULT 27
#define ESTIMATOR_STATUS_CS_RNG_FAULT 28
/*----------------------------------typedef-----------------------------------*/
union ekf_solution_status {
    struct {
        uint16_t attitude           : 1; ///< 0 - True if the attitude estimate is good
        uint16_t velocity_horiz     : 1; ///< 1 - True if the horizontal velocity estimate is good
        uint16_t velocity_vert      : 1; ///< 2 - True if the vertical velocity estimate is good
        uint16_t pos_horiz_rel      : 1; ///< 3 - True if the horizontal position (relative) estimate is good
        uint16_t pos_horiz_abs      : 1; ///< 4 - True if the horizontal position (absolute) estimate is good
        uint16_t pos_vert_abs       : 1; ///< 5 - True if the vertical position (absolute) estimate is good
        uint16_t pos_vert_agl       : 1; ///< 6 - True if the vertical position (above ground) estimate is good
        uint16_t const_pos_mode     : 1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
        uint16_t pred_pos_horiz_rel : 1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
        uint16_t pred_pos_horiz_abs : 1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
        uint16_t gps_glitch         : 1; ///< 10 - True if the EKF has detected a GPS glitch
        uint16_t accel_error        : 1; ///< 11 - True if the EKF has detected bad accelerometer data
    } flags;
    uint16_t value;
};

union ekf_fault_status {
    struct {
        bool bad_mag_x         : 1; ///< 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
        bool bad_mag_y         : 1; ///< 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
        bool bad_mag_z         : 1; ///< 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
        bool bad_hdg           : 1; ///< 3 - true if the fusion of the heading angle has encountered a numerical error
        bool bad_mag_decl      : 1; ///< 4 - true if the fusion of the magnetic declination has encountered a numerical error
        bool bad_airspeed      : 1; ///< 5 - true if fusion of the airspeed has encountered a numerical error
        bool bad_sideslip      : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
        bool bad_optflow_X     : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
        bool bad_optflow_Y     : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
        bool bad_vel_N         : 1; ///< 9 - true if fusion of the North velocity has encountered a numerical error
        bool bad_vel_E         : 1; ///< 10 - true if fusion of the East velocity has encountered a numerical error
        bool bad_vel_D         : 1; ///< 11 - true if fusion of the Down velocity has encountered a numerical error
        bool bad_pos_N         : 1; ///< 12 - true if fusion of the North position has encountered a numerical error
        bool bad_pos_E         : 1; ///< 13 - true if fusion of the East position has encountered a numerical error
        bool bad_pos_D         : 1; ///< 14 - true if fusion of the Down position has encountered a numerical error
        bool bad_acc_bias      : 1; ///< 15 - true if bad delta velocity bias estimates have been detected
        bool bad_acc_vertical  : 1; ///< 16 - true if bad vertical accelerometer data has been detected
        bool bad_acc_clipping  : 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing)
    } flags;
    uint32_t value;
};

// define structure used to communicate innovation test failures
union innovation_fault_status {
    struct {
        bool reject_hor_vel   : 1; ///< 0 - true if horizontal velocity observations have been rejected
        bool reject_ver_vel   : 1; ///< 1 - true if vertical velocity observations have been rejected
        bool reject_hor_pos   : 1; ///< 2 - true if horizontal position observations have been rejected
        bool reject_ver_pos   : 1; ///< 3 - true if true if vertical position observations have been rejected
        bool reject_mag_x     : 1; ///< 4 - true if the X magnetometer observation has been rejected
        bool reject_mag_y     : 1; ///< 5 - true if the Y magnetometer observation has been rejected
        bool reject_mag_z     : 1; ///< 6 - true if the Z magnetometer observation has been rejected
        bool reject_yaw       : 1; ///< 7 - true if the yaw observation has been rejected
        bool reject_airspeed  : 1; ///< 8 - true if the airspeed observation has been rejected
        bool reject_sideslip  : 1; ///< 9 - true if the synthetic sideslip observation has been rejected
        bool reject_hagl      : 1; ///< 10 - true if the height above ground observation has been rejected
        bool reject_optflow_X : 1; ///< 11 - true if the X optical flow observation has been rejected
        bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected
    } flags;
    uint16_t value;
};

// publish the status of various GPS quality checks
union gps_check_fail_status {
    struct {
        uint16_t fix    : 1; ///< 0 - true if the fix type is insufficient (no 3D solution)
        uint16_t nsats  : 1; ///< 1 - true if number of satellites used is insufficient
        uint16_t pdop   : 1; ///< 2 - true if position dilution of precision is insufficient
        uint16_t hacc   : 1; ///< 3 - true if reported horizontal accuracy is insufficient
        uint16_t vacc   : 1; ///< 4 - true if reported vertical accuracy is insufficient
        uint16_t sacc   : 1; ///< 5 - true if reported speed accuracy is insufficient
        uint16_t hdrift : 1; ///< 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
        uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground)
        uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
        uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive
    } flags;
    uint16_t value;
};

// bitmask containing filter control status
union filter_control_status {
    struct {
        uint64_t tilt_align              : 1; ///< 0 - true if the filter tilt alignment is complete
        uint64_t yaw_align               : 1; ///< 1 - true if the filter yaw alignment is complete
        uint64_t gps                     : 1; ///< 2 - true if GPS measurement fusion is intended
        uint64_t opt_flow                : 1; ///< 3 - true if optical flow measurements fusion is intended
        uint64_t mag_hdg                 : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended
        uint64_t mag_3D                  : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is intended
        uint64_t mag_dec                 : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended
        uint64_t in_air                  : 1; ///< 7 - true when the vehicle is airborne
        uint64_t wind                    : 1; ///< 8 - true when wind velocity is being estimated
        uint64_t baro_hgt                : 1; ///< 9 - true when baro height is being fused as a primary height reference
        uint64_t rng_hgt                 : 1; ///< 10 - true when range finder height is being fused as a primary height reference
        uint64_t gps_hgt                 : 1; ///< 11 - true when GPS height is being fused as a primary height reference
        uint64_t ev_pos                  : 1; ///< 12 - true when local position data fusion from external vision is intended
        uint64_t ev_yaw                  : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended
        uint64_t ev_hgt                  : 1; ///< 14 - true when height data from external vision measurements is being fused
        uint64_t fuse_beta               : 1; ///< 15 - true when synthetic sideslip measurements are being fused
        uint64_t mag_field_disturbed     : 1; ///< 16 - true when the mag field does not match the expected strength
        uint64_t fixed_wing              : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
        uint64_t mag_fault               : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
        uint64_t fuse_aspd               : 1; ///< 19 - true when airspeed measurements are being fused
        uint64_t gnd_effect              : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
        uint64_t rng_stuck               : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
        uint64_t gps_yaw                 : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
        uint64_t mag_aligned_in_flight   : 1; ///< 23 - true when the in-flight mag field alignment has been completed
        uint64_t ev_vel                  : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
        uint64_t synthetic_mag_z         : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
        uint64_t vehicle_at_rest         : 1; ///< 26 - true when the vehicle is at rest
        uint64_t gps_yaw_fault           : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
        uint64_t rng_fault               : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
        uint64_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
        uint64_t wind_dead_reckoning     : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
        uint64_t rng_kin_consistent      : 1; ///< 31 - true when the range finder kinematic consistency check is passing
        uint64_t fake_pos                : 1; ///< 32 - true when fake position measurements are being fused
        uint64_t fake_hgt                : 1; ///< 33 - true when fake height measurements are being fused
        uint64_t gravity_vector          : 1; ///< 34 - true when gravity vector measurements are being fused
        uint64_t mag                     : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
        uint64_t ev_yaw_fault            : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
    } flags;
    uint64_t value;
};

/** @ 
  * @brief  
  */
typedef struct {
    uint64_t timestamp;
    uint64_t timestamp_sample;
    uint64_t control_mode_flags;
    float output_tracking_error[3];
    uint32_t filter_fault_flags;
    float pos_horiz_accuracy;
    float pos_vert_accuracy;
    float mag_test_ratio;
    float vel_test_ratio;
    float pos_test_ratio;
    float hgt_test_ratio;
    float tas_test_ratio;
    float hagl_test_ratio;
    float beta_test_ratio;
    float time_slip;
    uint32_t accel_device_id;
    uint32_t gyro_device_id;
    uint32_t baro_device_id;
    uint32_t mag_device_id;
    uint16_t gps_check_fail_flags;
    uint16_t innovation_check_flags;
    uint16_t solution_status_flags;
    uint8_t reset_count_vel_ne;
    uint8_t reset_count_vel_d;
    uint8_t reset_count_pos_ne;
    uint8_t reset_count_pod_d;
    uint8_t reset_count_quat;
    uint8_t health_flags;
    uint8_t timeout_flags;
    bool pre_flt_fail_innov_heading;
    bool pre_flt_fail_innov_vel_horiz;
    bool pre_flt_fail_innov_vel_vert;
    bool pre_flt_fail_innov_height;
    bool pre_flt_fail_mag_field_disturbed;
}uitc_estimator_status;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



